# 发送端代码 - 使用CircuitPython
# 功能：读取控制器输入，通过WiFi发送控制信号

import board
import analogio
import wifi
import socketpool
import time
import os

# WiFi配置
WIFI_SSID = "esp32_motor_ap"
WIFI_PASSWORD = "12345678"
RECEIVER_IP = "192.168.4.1"
UDP_PORT = 4210

# 控制器输入引脚（电位器/摇杆）
controller = analogio.AnalogIn(board.IO34)

def connect_wifi():
    """连接WiFi网络"""
    print("正在连接WiFi...")
    wifi.radio.start_scanning_networks()
    
    # 扫描并连接WiFi
    for network in wifi.radio.start_scanning_networks():
        if network.ssid == WIFI_SSID:
            wifi.radio.connect(WIFI_SSID, WIFI_PASSWORD)
            break
    
    wifi.radio.stop_scanning_networks()
    
    if wifi.radio.connected:
        print(f"WiFi连接成功！IP地址: {wifi.radio.ipv4_address}")
        return True
    else:
        print("WiFi连接失败！")
        return False

def read_controller():
    """读取控制器输入并转换为方向和速度"""
    # 读取ADC值 (0-65535)
    adc_value = controller.value
    
    # 计算方向和速度
    # 以32768为中点，左转为负，右转为正
    if adc_value > 34000:  # 右转
        direction = 1
        speed = int((adc_value - 32768) * 255 / (65535 - 32768))
    elif adc_value < 31500:  # 左转
        direction = -1
        speed = int((31500 - adc_value) * 255 / 31500)
    else:  # 静止区域
        direction = 0
        speed = 0
    
    return direction, speed

def send_control_data(direction, speed):
    """发送控制数据到接收端"""
    try:
        # 创建UDP socket
        pool = socketpool.SocketPool(wifi.radio)
        sock = pool.socket()
        
        # 组装数据包
        data = f"{direction},{speed}"
        
        # 发送UDP数据包
        sock.sendto(data.encode(), (RECEIVER_IP, UDP_PORT))
        sock.close()
        
        print(f"发送数据: 方向={direction}, 速度={speed}")
        
    except Exception as e:
        print(f"发送数据失败: {e}")

def main():
    """主程序"""
    print("ESP32电机遥控器启动...")
    
    # 连接WiFi
    if not connect_wifi():
        print("无法连接WiFi，程序退出")
        return
    
    print("开始发送控制信号...")
    
    while True:
        try:
            # 读取控制器输入
            direction, speed = read_controller()
            
            # 发送控制数据
            send_control_data(direction, speed)
            
            # 延时50ms (20Hz发送频率)
            time.sleep(0.05)
            
        except KeyboardInterrupt:
            print("程序被用户中断")
            break
        except Exception as e:
            print(f"程序错误: {e}")
            time.sleep(1)

if __name__ == "__main__":
    main()